<html>
  <head>
    <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
    <link rel="stylesheet" href="http://www.petercorke.com/RVC/common/toolboxhelp.css">
    <title>M-File Help: ParticleFilter</title>
  </head>
  <body>
  <table border="0" cellspacing="0" width="100%">
    <tr class="subheader">
      <td class="headertitle">M-File Help: ParticleFilter</td>
      <td class="subheader-left"><a href="matlab:open ParticleFilter">View code for ParticleFilter</a></td>
    </tr>
  </table>
<h1>ParticleFilter</h1><p><span class="helptopic">Particle filter class</span></p><p>
Monte-carlo based localisation for estimating vehicle pose based on
odometry and observations of known landmarks.

</p>
<h2>Methods</h2>
<table class="list">
  <tr><td style="white-space: nowrap;" class="col1"> run</td> <td>run the particle filter</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> plot_xy</td> <td>display estimated vehicle path</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> plot_pdf</td> <td>display particle distribution</td></tr>
</table>
<h2>Properties</h2>
<table class="list">
  <tr><td style="white-space: nowrap;" class="col1"> robot</td> <td>reference to the robot object</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> sensor</td> <td>reference to the sensor object</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> history</td> <td>vector of structs that hold the detailed information from
each time step</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> nparticles</td> <td>number of particles used</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> x </td> <td>particle states; nparticles x 3</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> weight</td> <td>particle weights; nparticles x 1</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> x_est</td> <td>mean of the particle population</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> std</td> <td>standard deviation of the particle population</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> Q </td> <td>covariance of noise added to state at each step</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> L </td> <td>covariance of likelihood model</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> w0</td> <td>offset in likelihood model</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> dim</td> <td>maximum xy dimension</td></tr>
</table>
<h2>Example</h2>
<p>
Create a landmark map

</p>
<pre style="width: 90%%;" class="examples">
map&nbsp;=&nbsp;Map(20);
</pre>
<p>
and a vehicle with odometry covariance and a driver

</p>
<pre style="width: 90%%;" class="examples">
W&nbsp;=&nbsp;diag([0.1,&nbsp;1*pi/180].^2);
veh&nbsp;=&nbsp;Vehicle(W);
veh.add_driver(&nbsp;RandomPath(10)&nbsp;);
</pre>
<p>
and create a range bearing sensor

</p>
<pre style="width: 90%%;" class="examples">
R&nbsp;=&nbsp;diag([0.005,&nbsp;0.5*pi/180].^2);
sensor&nbsp;=&nbsp;RangeBearingSensor(veh,&nbsp;map,&nbsp;R);
</pre>
<p>
For the particle filter we need to define two covariance matrices.  The
first is is the covariance of the random noise added to the particle
states at each iteration to represent uncertainty in configuration.

</p>
<pre style="width: 90%%;" class="examples">
Q&nbsp;=&nbsp;diag([0.1,&nbsp;0.1,&nbsp;1*pi/180]).^2;
</pre>
<p>
and the covariance of the likelihood function applied to innovation

</p>
<pre style="width: 90%%;" class="examples">
L&nbsp;=&nbsp;diag([0.1&nbsp;0.1]);
</pre>
<p>
Now construct the particle filter

</p>
<pre style="width: 90%%;" class="examples">
pf&nbsp;=&nbsp;ParticleFilter(veh,&nbsp;sensor,&nbsp;Q,&nbsp;L,&nbsp;1000);
</pre>
<p>
which is configured with 1000 particles.  The particles are initially
uniformly distributed over the 3-dimensional configuration space.

</p>
<p>
We run the simulation for 1000 time steps

</p>
<pre style="width: 90%%;" class="examples">
pf.run(1000);
</pre>
<p>
then plot the map and the true vehicle path

</p>
<pre style="width: 90%%;" class="examples">
map.plot();
veh.plot_xy('b');
</pre>
<p>
and overlay the mean of the particle cloud

</p>
<pre style="width: 90%%;" class="examples">
pf.plot_xy('r');
</pre>
<p>
We can plot the standard deviation against time

</p>
<pre style="width: 90%%;" class="examples">
plot(pf.std(1:100,:))
</pre>
<p>
The particles are a sampled approximation to the PDF and we can display
this as

</p>
<pre style="width: 90%%;" class="examples">
pf.plot_pdf()
</pre>
<h2>Acknowledgement</h2>
<p>
Based on code by Paul Newman, Oxford University,
http://www.robots.ox.ac.uk/~pnewman

</p>
<h2>Reference</h2>
<p>
Robotics, Vision &amp; Control,
Peter Corke,
Springer 2011

</p>
<h2>See also</h2>
<p>
<a href="Vehicle.html">Vehicle</a>, <a href="RandomPath.html">RandomPath</a>, <a href="RangeBearingSensor.html">RangeBearingSensor</a>, <a href="Map.html">Map</a>, <a href="EKF.html">EKF</a></p>
<hr>
<a name="ParticleFilter"><h1>ParticleFilter.ParticleFilter</h1></a>
<p><span class="helptopic">Particle filter constructor</span></p><p>
<strong>pf</strong> = <span style="color:red">ParticleFilter</span>(<strong>vehicle</strong>, <strong>sensor</strong>, <strong>q</strong>, <strong>L</strong>, <strong>np</strong>, <strong>options</strong>) is a particle
filter that estimates the state of the <strong>vehicle</strong> with a landmark sensor
<strong>sensor</strong>.  <strong>q</strong> is the covariance of the noise added to the particles
at each step (diffusion), <strong>L</strong> is the covariance used in the
sensor likelihood model, and <strong>np</strong> is the number of particles.

</p>
<h2>Options</h2>
<table class="list">
  <tr><td style="white-space: nowrap;" class="col1"> 'verbose'</td> <td>Be verbose.</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> 'private'</td> <td>Use private random number stream.</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> 'reset'</td> <td>Reset random number stream.</td></tr>
  <tr><td style="white-space: nowrap;" class="col1">'seed', S</td> <td>Set the initial state of the random number stream.  S must
be a proper random number generator state such as saved in
the seed0 property of an earlier run.</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> 'nohistory'</td> <td>Don't save history.</td></tr>
  <tr><td style="white-space: nowrap;" class="col1"> 'x0'</td> <td>Initial particle states (Nx3)</td></tr>
</table>
<h2>Notes</h2>
<ul>
  <li>ParticleFilter subclasses Handle, so it is a reference object.</li>
  <li>If initial particle states not given they are set to a uniform
distribution over the map, essentially the kidnapped robot problem
which is quite unrealistic.</li>
  <li>Initial particle weights are always set to unity.</li>
  <li>The 'private' option creates a private random number stream for the
methods rand, randn and randi.  If not given the global stream is used.</li>
</ul>
<h2>See also</h2>
<p>
<a href="Vehicle.html">Vehicle</a>, <a href="Sensor.html">Sensor</a>, <a href="RangeBearingSensor.html">RangeBearingSensor</a>, <a href="Map.html">Map</a></p>
<hr>
<a name="char"><h1>ParticleFilter.char</h1></a>
<p><span class="helptopic">Convert to string</span></p><p>
PF.<span style="color:red">char</span>() is a string representing the state of the <span style="color:red">ParticleFilter</span>
object in human-readable form.

</p>
<h2>See also</h2>
<p>
<a href="ParticleFilter.display.html">ParticleFilter.display</a></p>
<hr>
<a name="display"><h1>ParticleFilter.display</h1></a>
<p><span class="helptopic">Display status of particle filter object</span></p><p>
PF.<span style="color:red">display</span>() displays the state of the <span style="color:red">ParticleFilter</span> object in
human-readable form.

</p>
<h2>Notes</h2>
<ul>
  <li>This method is invoked implicitly at the command line when the result
of an expression is a ParticleFilter object and the command has no trailing
semicolon.</li>
</ul>
<h2>See also</h2>
<p>
<a href="ParticleFilter.char.html">ParticleFilter.char</a></p>
<hr>
<a name="init"><h1>ParticleFilter.init</h1></a>
<p><span class="helptopic">Initialize the particle filter</span></p><p>
PF.<span style="color:red">init</span>() initializes the particle distribution and clears the
history.

</p>
<h2>Notes</h2>
<ul>
  <li>If initial particle states were given to the constructor the states are
set to this value, else a random distribution over the map is used.</li>
  <li>Invoked by the run() method.</li>
</ul>
<hr>
<a name="plot_pdf"><h1>ParticleFilter.plot_pdf</h1></a>
<p><span class="helptopic">Plot particles as a PDF</span></p><p>
PF.<span style="color:red">plot_pdf</span>() plots a sparse PDF as a series of vertical line
segments of height equal to particle weight.

</p>
<hr>
<a name="plot_xy"><h1>ParticleFilter.plot_xy</h1></a>
<p><span class="helptopic">Plot vehicle position</span></p><p>
PF.<span style="color:red">plot_xy</span>() plots the estimated vehicle path in the xy-plane.

</p>
<p>
PF.<span style="color:red">plot_xy</span>(<strong>ls</strong>) as above but the optional line style arguments
<strong>ls</strong> are passed to plot.

</p>
<hr>
<a name="run"><h1>ParticleFilter.run</h1></a>
<p><span class="helptopic">Run the particle filter</span></p><p>
PF.<span style="color:red">run</span>(<strong>n</strong>, <strong>options</strong>) runs the filter for <strong>n</strong> time steps.

</p>
<h2>Options</h2>
<table class="list">
  <tr><td style="white-space: nowrap;" class="col1"> 'noplot'</td> <td>Do not show animation.</td></tr>
</table>
<h2>Notes</h2>
<ul>
  <li>All previously estimated states and estimation history is
cleared.</li>
</ul>
<hr>

<table border="0" width="100%" cellpadding="0" cellspacing="0">
  <tr class="subheader" valign="top"><td>&nbsp;</td></tr></table>
<p class="copy">&copy; 1990-2014 Peter Corke.</p>
</body></html>